/*
 * main.c
 *
 *  Created on: Apr 6, 2012
 *      Author: jeorge_clowner
 */

#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>

#include <pthread.h>

#include <malloc.h>

#include <math.h>

#include "../include/sensors/LSM303DLH_m.h"
#include "../include/sensors/lsm303DLH_a.h"
#include "../include/sensors/l3g4200d.h"
#include "../include/sensors/CHR_GPS.h"

static pthread_mutex_t my_mutex;

typedef struct{
	mag_sensor *magnetometer;
	acc_sensor *accelerometer;
	gyr_sensor *gyrometer;
	GPS_Struct *gps;
}sensors;

/* changer d'emplacement les tâches, les mettre dans les fichiers correspondant */

void *thread_process_init_sensors (void* sensors_data)
{
	pthread_mutex_lock (&my_mutex);
	sensors *data = sensors_data;
	/**************************************************/
	/* ATTENTION CALIBRATION NECESSITE TACHE acq_data */
	/**************************************************/
	/* demarrer le GPS */
	/* regarder la configuration dans un fichier texte si configurable */
	/* demarrer l'acceleromètre regarder si configuration et calibration au quel cas le calibrer et reconfigurer */
	printf("process init and calib MEMs\n");
	Acc_Init(data->accelerometer);
	/* demarrer le magnétomètre regarder si configuration et calibration au quel cas le calibrer et reconfigurer */
	Mag_Init(data->magnetometer);
	data->magnetometer->config.rate=LSM303DLH_M_RATE_30_00;
	Mag_write_config(data->magnetometer);
	/* demarrer le gyromètre regarder si configuration et calibration au quel cas le calibrer et reconfigurer */
	Gyr_Init(data->gyrometer);
	pthread_mutex_unlock (&my_mutex);
	pthread_exit (0);
	return (NULL);
}

void *thread_process_acq_data (void* sensors_data)
{
	/* faire un timer ou tester timer */
	/* si timer ok then */
		pthread_mutex_lock (&my_mutex);
		sensors *data = sensors_data;

		Mag_read_data(data->magnetometer);
		Acc_read_data(data->accelerometer);
		Gyr_read_data(data->gyrometer);
		//read_buf_gps(data->gps);
		printf("Mag:\nX:%lf\tY:%lf\tZ:%lf\n\n",data->magnetometer->raw.axis[0],data->magnetometer->raw.axis[1],data->magnetometer->raw.axis[2]);
		printf("Acc:\nX:%lf\tY:%lf\tZ:%lf\n\n",data->accelerometer->raw.axis[0],data->accelerometer->raw.axis[1],data->accelerometer->raw.axis[2]);
		printf("Gyr:\nX:%d\tY:%d\tZ:%d\n\n",data->gyrometer->raw.axis[0],data->gyrometer->raw.axis[1],data->gyrometer->raw.axis[2]);

		double pitch,roll, heading,Mx, My, Mz;
		pitch=asin(-data->accelerometer->raw.axis[0]);
		roll=asin(data->accelerometer->raw.axis[1]/cos(pitch));
		Mx=data->magnetometer->raw.axis[0]*cos(pitch)+data->magnetometer->raw.axis[2]*sin(pitch);
		My=data->magnetometer->raw.axis[0]*sin(roll)*sin(pitch)+data->magnetometer->raw.axis[1]*cos(roll)-data->magnetometer->raw.axis[2]*sin(roll)*cos(pitch);
		Mz=-data->magnetometer->raw.axis[0]*cos(roll)*sin(pitch)+data->magnetometer->raw.axis[1]*sin(roll)+data->magnetometer->raw.axis[2]*cos(roll)*cos(pitch);
		if (Mx>0 && My>=0)
			heading=atan(My/Mx)*180/M_PI;
		else if(Mx<0)
			heading=180+atan(My/Mx)*180/M_PI;
		else if(Mx>0 && My<=0)
			heading=360+atan(My/Mx)*180/M_PI;
		else if(Mx==0 && My<0)
			heading=90;
		else if(Mx==0 && My>0)
			heading=270;

		pthread_mutex_unlock (&my_mutex);
	pthread_exit (0);
	return (NULL);
}

//intégrer cette fonctionnalité dans le thread d'init car les taches s'exécutent l'une après l'autre
void *thread_process_cal_MEMs (void* sensors_data)
{
		pthread_mutex_lock (&my_mutex);
		sensors *data = sensors_data;
		//raw_data_cal(data->magnetometer, data->accelerometer, 0);
		//A_1_calibration(data->magnetometer, data->accelerometer, 0);
		pthread_mutex_unlock (&my_mutex);
	pthread_exit (0);
	return(NULL);
}

void *thread_process_filter_data (void* sensors_data)
{
	/* faire un timer ou tester timer */
	/* si timer ok then */
		pthread_mutex_lock (&my_mutex);
//		sensors *data = sensors_data;
		/* filtre de kalman */
		pthread_mutex_unlock (&my_mutex);
	pthread_exit (0);
	return(NULL);
}

/* Todo modifier le type */
/* permet de lire les données enregistrées dans le fichier et décrire toutes modification dans ce meme fichier à la fin de l'appli */
//sensors *sensors_data;
/* todo choisir entre le pointeur de struture pour la durée du soft ou alors structure qui permet de récypérer
        les infos issues du fichier de configuration stocké en mémoire*/
/*
mag_sensor mag_data;
acc_sensor acc_data;
struct gyr_sensor gyr_data;
struct GPS_Struct GPS_data;
*/

int main(void)
{
	printf("hello world\n");

	pthread_t th0, th1, th2;

	void *ret;
	/* former une seule structure ?? */

	sensors *sensors_data;
	sensors_data = (sensors*)malloc(sizeof(sensors));

	mag_sensor magnetometer;
	acc_sensor accelerometer;
	gyr_sensor gyrometer;
	GPS_Struct gps;
	sensors_data->magnetometer=&magnetometer;
	sensors_data->accelerometer=&accelerometer;
	sensors_data->gyrometer=&gyrometer;
	sensors_data->gps=&gps;

	/* tache init des MEMs, GPS... + vérifie la calibration de ces derniers (dans un fichier texte */

	if (pthread_create (&th0, NULL, thread_process_init_sensors, (sensors*)sensors_data) < 0) {
		fprintf (stderr, "pthread_create error for thread 0\n");
		exit (1);
	}

	while(1)
	{
		/* verifier si besoin de lancer la tache avant ou pas avec caliration...*/

		if (pthread_create (&th1, NULL, thread_process_acq_data, (sensors*)sensors_data) < 0) {
			fprintf (stderr, "pthread_create error for thread 1\n");
			exit (1);
		}

/*
		if (pthread_create (&th2, NULL, thread_process_cal_MEMs, (sensors*)sensors_data) < 0) {
					fprintf (stderr, "pthread_create error for thread 1\n");
					exit (1);
		}
*/
		/*
		if (pthread_create (&th3, NULL, thread_process_filter_data, (sensors*)sensors_data) < 0) {
			fprintf (stderr, "pthread_create error for thread 2\n");
			exit (1);
		}
		*/
		sleep(1);
		(void)pthread_join (th0, &ret);
		(void)pthread_join (th1, &ret);
		//(void)pthread_join (th2, &ret);

	}
	free(sensors_data);
	return 0;
}
